using System;
using System.Collections.Generic;
using UnityEngine;

public class FlyTo : MonoBehaviour
{
    public float Speed = 20;
    public float Accuracy = 1;
    public float RotSpeed = 15;

    private int currentWP = 0;
    private Vector3 goal;
    
    public GameObject octree;
    Graph graph;
    Octree ot;
    List<Node> pathList = new List<Node>();

    public GameObject goalPosition;

    private void Start()
    {
        Invoke("Navigate",1);
    }

    void Navigate()
    {
        graph = octree.GetComponent<CreateOctree>().wayPoints;
        ot = octree.GetComponent<CreateOctree>()._ot;
    }
    // 添加这个方法来找到最近的节点
    Node FindNearestNode()
    {
        float minDistance = float.MaxValue;
        Node nearestNode = null;

        foreach (Node node in graph.nodes)
        {
            float distance = Vector3.Distance(node.octreeNode.nodeBounds.center, transform.position);
            if (distance < minDistance)
            {
                minDistance = distance;
                nearestNode = node;
            }
        }

        return nearestNode;
    }
    void NavigateTo(int destination,Node finalGoal)
    {
        Node destinationNode = graph.FindNode(destination);
        graph.AStar(FindNearestNode().octreeNode, destinationNode.octreeNode, pathList);
        currentWP = 0;
        pathList.Add(finalGoal);
    }

    public int getPathLength()
    {
        return pathList.Count;
    }

    public OctreeNode getPathPoint(int _index)
    {
        return pathList[_index].octreeNode;
    }

    private void Update()
    {
        if(ot == null)return;
        if (Input.GetKeyDown(KeyCode.Space))
        {
            Vector3 pos = goalPosition.transform.position;
            int i = ot.AddDestination(pos);
            if (i==-1)
            {
                Debug.Log("Destination not found in octree");
                return;
            }

            Node finalGoal = new Node(new OctreeNode(new Bounds(pos, new Vector3(1, 1, 1)),1,null));
            NavigateTo(i,finalGoal);
        }
    }

    private void LateUpdate()
    {
        if (graph == null)
        {
            return;
        }

        if (getPathLength() == 0 || currentWP == getPathLength())
        {
            return;
        }

        if (Vector3.Distance(getPathPoint(currentWP).nodeBounds.center, this.transform.position) <= Accuracy)
        {
            currentWP++;
        }

        if (currentWP < getPathLength())
        {
            goal = getPathPoint(currentWP).nodeBounds.center;
            Vector3 lookAtGoal = new Vector3(goal.x,goal.y,goal.z);
            Vector3 direction = lookAtGoal - this.transform.position;
            this.transform.rotation = Quaternion.Slerp(this.transform.rotation, Quaternion.LookRotation(direction), RotSpeed * Time.deltaTime);
            this.transform.Translate(0,0,Speed * Time.deltaTime);
        }
        else
        {
            if (getPathLength() == 0)
            {
                return;
            }
        }
    }
    
}